// Copyright FYT 邹承甫 2022

#ifndef POST_PROCESSOR__TRACKER_HPP
#define POST_PROCESSOR__TRACKER_HPP

#include <deque>
#include <memory>
#include <vector>

#include <ros/ros.h>
#include <Eigen/Core>

#include "post_processor/armor.hpp"
#include "post_processor/kalman_filter.hpp"

namespace post_processor {

namespace tracker {
typedef enum { LOST, TRACKING } state;
}

class Tracker {
 public:
  using Ptr = std::unique_ptr<Tracker>;
  Tracker(const std::vector<double> &q_vec, const std::vector<double> &p_vec,
          const std::vector<double> &f_vec, const std::vector<double> &h_vec,
          const std::vector<double> &r_vec, const double &max_match_distance,
          const double &max_spinning_angle, const int &max_queue_length,
          const bool &is_debug);

 public:
  Eigen::VectorXd update(const std::vector<Armor> &armors);

  void set_predict_time(const double &t);

 private:
  void initialize(const std::vector<Armor> &armors);

  static bool antiSpinning(const std::vector<Armor> &armors, const Armor &selected,
                    Armor &new_target);
  bool debug_;

  KalmanFilter::Ptr kf_;
  std::deque<Armor> history_queue_;
  ros::Time last_time;
  Eigen::VectorXd last_state;

  double max_match_thresh_;
  double predict_time_;
  int state_;
  int tracking_id_;
  int lost_cnt_;
  int max_queue_size_;
  double max_angle_;
};

}  // namespace post_processor

#endif  // POST_PROCESSOR__TRACKER_HPP